
#include "funcdef.h"

//double BMA180_x,BMA180_y,BMA180_z,BMA180_T = 0;
//short rBMA180_x,rBMA180_y,rBMA180_z,rBMA180_T = 0;
//double roll, pitch = 0;

const int ACC_SCALE[7] = {2,3,4,6,8,16,32};
int ACC_CURRENT_BW = 0;
int ACC_CURRENT_SCALE = 0;
Uint8 ACC_CURRENT_SCALE_INDEX = 0;
/*ACC_T0 is the compensation coefficient under normal temperature*/
float ACC_T0[12] = {0.995874800467359,-0.0100396177074092,0.00538480592891706,
					0.0138450163674559,0.981131835310753,0.0166294739648584,
					0.00265684269805071,0.00169674404071982,0.988371932453212,
					0.000945977565383199,0.0483449152460015,-0.00718976710309092};
/*ACC_T is the temperature compensation coefficient*/
float ACC_T[12] = { 3.38505389254410e-07,5.39394109753295e-07,1.69371788601543e-06,
					2.61790277988253e-07,4.41890218457419e-05,0.000100627940990684,
					-4.41224490195422e-05,0.000838425880779328,0.00171401145315776,
					-0.00161157530664348,0.00495513780094281,0.0130259111392230};

int BMA180_init(TSK_Ctrl_b *tsk);
static int sync_message_rec(QUE_Handle queue, Msg * message);

/*--------------BMA180_sample_PRD()-----------------------------
 * This task is set up to run periodically by pending on BMA180_sem,
 * which is update by BMA180_sample_PRD PRD module.
 *------------------------------------------------ ------*/
void BMA180_sample_tsk(void)
{
	double data[4] = {0x00,0x00,0x00,0x00};
	float temp[4] = {0x00,0x00,0x00,0x00};
	short rdata[4] = {0x00,0x00,0x00,0x00};
	float data_after_calb[3] = {0x00,0x00,0x00};
	int error = 0;

	Msg message;
	QUE_Handle reply_queue;
	SEM_Handle msg_sem;

	TSK_Ctrl_b *self = &(SCB.TCB[TSK_BMA180]);

	SEM_pend(&BMA180_sem, SYS_FOREVER);
	// Check if the sensor has been initialized
	while(self->status != INITIALISED){
		error = BMA180_init(self);
		if(error){
#ifdef _DEBUG
			LOG_printf(self->trace, "BMA180 INIT failed: %d\n", error);
#endif
			//Reset IIC bus
			error = GIO_control(self->inhd,I2c_IOCTL_DEVICE_SOFT_RESET,NULL);
#ifdef _DEBUG
			if(error){
				LOG_printf(self->trace, "RESET IIC failed: %d\n", error);
			}else{
				LOG_printf(self->trace, "RESET IIC succeed\n");
			}
#endif
		}
	}


	vTskTime(self,RESET_TIME);
#ifdef _DEBUG
	/* Initialize task's STS object to current time */
	TSK_settime(TSK_self());
#endif

	/* Sensor initialized, perform sampling operations */
	for(;;){
		SEM_pend(&BMA180_sem, SYS_FOREVER);

		//Benchmark
		//vTskTime(self,SET_PREV_TIME);
#ifdef _DEBUG
		STS_set(&sts_BMA180, SCB.TIME_STAMP);
#endif

		/* Sensor initialized, perform sampling operations */
		error =  BMA180_GET_RDATA(self->inhd, self->outhd,rdata);
		if(!error){
			vNavData.rBMA180_x = rdata[0];
			vNavData.rBMA180_y = rdata[1];
			vNavData.rBMA180_z = rdata[2];
			vNavData.rBMA180_T = rdata[3];
			BMA180_SCALE_DATA(rdata,data,ACC_CURRENT_SCALE);
			vNavData.BMA180_T = data[3];
			data[3] = 1;
			mat_db_to_ft(4, data, temp);
			MatMul(1, 4, 3, temp, ACC_T0, data_after_calb);	
			vNavData.BMA180_x = data_after_calb[0];
			vNavData.BMA180_y = data_after_calb[1];
			vNavData.BMA180_z = data_after_calb[2];
			
			vNavData.roll = BMA180_GETROLL(vNavData.BMA180_x,vNavData.BMA180_y,vNavData.BMA180_z);
			vNavData.pitch = BMA180_GETPITCH(vNavData.BMA180_x,vNavData.BMA180_y,vNavData.BMA180_z);
		}else{
#ifdef _DEBUG
			LOG_printf(self->trace, "BMA180_GETRAWDATA failed: %d\n", error);
#endif
			error = GIO_control(self->inhd,I2c_IOCTL_DEVICE_SOFT_RESET,NULL);
#ifdef _DEBUG
			if(error){
				LOG_printf(self->trace, "RESET IIC failed: %d\n", error);
			}else{
				LOG_printf(self->trace, "RESET IIC succeed\n");
			}
#endif
		}

		if(sync_message_rec(self->queue, &message)){
			/* receive message from queue */
			if(message->sender >= NUM_TSKS){
				//msg error
#ifdef _DEBUG
				LOG_printf(self->trace, "ERROR message from sender: %d\n", message->sender);
#endif
			}else{
				if(ACC_SET_BW == message->cmd_1){

					error = BMA180_SET_BW(self->inhd,self->outhd, message->cmd_2);

				}else if(ACC_SET_RANGE == message->cmd_1){

					error = BMA180_SET_RANGE(self->inhd,self->outhd,message->cmd_2);

				}else{

					error = INVALID_CMD;
#ifdef _DEBUG
					LOG_printf(self->trace, "Invalid cmd_1 %d : cmd_2 %d\n", message->cmd_1, message->cmd_2);
#endif
				}

				if(!error){
					//update magnetometer current setting parameters
					switch(message->cmd_1){
					case ACC_SET_BW:

						ACC_CURRENT_BW = message->cmd_2;

						break;
					case ACC_SET_RANGE:
						ACC_CURRENT_SCALE_INDEX = message->cmd_2;
						ACC_CURRENT_SCALE = ACC_SCALE[message->cmd_2];

						break;
					}
#ifdef _DEBUG
					LOG_printf(self->trace, "TSK_BMA180: set acc successfully\n");
#endif
				}
				/* Finish and reply to sender */
				message->status = error;
				/* copy the queue semaphore and reply queue*/
				msg_sem = message->sem;
				reply_queue = message->receipt;
				/* put message back onto sender's queue */
				QUE_put(reply_queue, message);
				/* signal the sender on completion */
				SEM_post(msg_sem);
			}
		}

		//Benchmark
		vTskTime(self,DELTA_TIME);
#ifdef _DEBUG
		STS_delta(&sts_BMA180,SCB.TIME_STAMP);
		/* Get time difference and
		add it to task's STS object */
		TSK_deltatime(TSK_self());
#endif
		TSK_yield();
	}
}

/* This function receive one small size message from queue synchronously */
static int sync_message_rec(QUE_Handle queue, Msg * message){
	if (QUE_empty(queue)) {
		return 0;
	}
	/* dequeue message */
	*message = QUE_get(queue);
	/* free msg - only done by sender if send in synchronize faction */
	//MEM_free(0, msg, sizeof(MsgObj));
	return 1;
}


/*--------------BMA180_init()-----------------------------
 * This function performs deviceID check
 * and read out initial values
 * from BMA180 accelerometer.
 *------------------------------------------------ ------*/
int BMA180_init(TSK_Ctrl_b *tsk){
	int error = 0;
	//Connect to BMA180
	error = BMA180_CONNECT(tsk->inhd,tsk->outhd);
	if(error){
		//DEBUG
#ifdef _DEBUG
		LOG_printf(tsk->trace, "BMA180_CONNECT failed: %d\n", error);
#endif
		return error;
	}
	error = BMA180_SET_RANGE(tsk->inhd,tsk->outhd,BMA180_2G);
	if(error){
		//DEBUG
#ifdef _DEBUG
		LOG_printf(tsk->trace, "BMA180_SET_RANGE failed: %d\n", error);
#endif
		return error;
	}
	ACC_CURRENT_SCALE_INDEX = BMA180_2G;
	ACC_CURRENT_SCALE = ACC_SCALE[BMA180_2G];

	error = BMA180_SET_BW(tsk->inhd,tsk->outhd,BMA180_150HZ);
	if(error){
		//DEBUG
#ifdef _DEBUG
		LOG_printf(tsk->trace, "BMA180_SET_BW failed: %d\n", error);
#endif
		return error;
	}
	ACC_CURRENT_BW	= BMA180_150HZ;
	tsk->status = INITIALISED;
	//DEBUG
	//tsk->pbuf[]	= "BMA180 Initialized\n\r";
	//tsk->len	= strlen(tsk->pbuf);
#ifdef _DEBUG
	LOG_printf(tsk->trace, "BMA180 Initialized Successfully\n");
#endif
	return error;
}


